/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright, Josep Bosch
*
*********************************************************************/

// Define DEBUG_OUTPUT to enable PRINTLN_DEBUG output when not using ROS.
// ROS debug level output is toggled at runtime using rxconsole.
//#define DEBUG_OUTPUT

// ================================================================== includes

#include <algorithm>
#include <iostream>
#include <sstream>
#include <string>

using namespace std;

#include <errno.h>
#include <float.h>
#include <limits.h>
#include <signal.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <stdlib.h>
#include <time.h>

#include <osl_core/ProcessWatcher.h>
#include <osl_core/ModuleCore.h>

// ========================================================== static variables

namespace osl_core
{

bool ModuleCore::runOnce = false;

ModuleCore * ModuleCore::activeCore = NULL;

// TODO: What to do about this?
// INI file name to use.
string ModuleCore::iniFilename = "../conf/AUV.ini";

// =================================================================== methods

// ---------------------------------------------------------------------------
ModuleCore::ModuleCore()
{
	m_standaloneProcessMode = false;
	m_initCalled = false;
	m_stopRequested = false;
	m_exitValue = -1;
	m_argc = 0;
	m_argv = NULL;
}

// ---------------------------------------------------------------------------
ModuleCore::~ModuleCore()
{

}

// ---------------------------------------------------------------------------
bool ModuleCore::coreInit()
{
	if (m_standaloneProcessMode) {
		m_diagPrefix = "";

		if (m_initCalled) {
			PRINTLN_ERR("ModuleCore::coreInit: attempt to call more than once!");
			return false;
		}

		m_initCalled = true;
	}
	else {
		m_diagPrefix = "Sub-";
	}

	//PRINTLN_INFO(m_diagPrefix << "ModuleCore: coreInit entered");

	// Only call ros::init if it is a standalone module, not a sub-module like an ActionCore.
	if (m_standaloneProcessMode) {
		// Initialise ROS.
		PRINTLN_INFO("Initialising ROS with args: ");
		for (int i=0; i<m_argc; ++i) {
			PRINTLN_INFO("  " << i << " : "  << m_argv[i]);
		}
		// We install our own signal handler, so we tell ROS not to use its own.
		// We aren't using ROS console output for now, so tell ROS not to broadcast that.
		ros::init(m_argc, m_argv, createROSNodeName(m_moduleName), ros::init_options::NoSigintHandler | ros::init_options::NoRosout);
	}

	// We want to create a ROS handle whether we're standalone or a sub-module.
	m_rosHandle = ros::NodeHandlePtr(new ros::NodeHandle());

	if (!checkROSClock()) return false;

	// Check if parameter indicates we should use SOA or not.
	if (!m_rosHandle->getParam("/use_soa", m_useSoa))
	{
		PRINTLN_INFO("No ROS parameter /using_soa, defaulting to false.");
		m_useSoa = false;
	}

	if (m_useSoa) {
		PRINTLN_INFO("SOA is enabled.");
		// Create the SOA matchmaker client.
		m_soaClient = boost::make_shared<matchmaker::Client>();
	}
	else {
		PRINTLN_INFO("SOA is disabled.");
	}

	// Transport hints for subscribing (earlier hints are preferred over later ones).
	m_rosHints = ros::TransportHints().udp().tcpNoDelay();

	// Load ini file. Has to be done separately from in main() below,
	// as the config file may be changed and the module restarted by
	// the watcher.
	if (!m_conf.initROSParam(m_rosHandle, "conf"))
	{
		// Can't do anything without configuration, so return.
		return false;
	}

	m_minWaitTime = DEFAULT_MIN_WAIT_TIME;

	// Call custom module init function (after potential ROS init).
	if (!init(m_argc, m_argv))
	{
		PRINTLN_ERR("ModuleCore: custom init failed for " << m_moduleName << ", exiting.");
		return false;
	}

	if (m_standaloneProcessMode) {
		// Install signal handler using pointer to this core object.
		// Not wanted for NessieAction.
		initSignals();
	}

	return true;
}

// ---------------------------------------------------------------------------
void ModuleCore::coreCleanup(void)
{
	PRINTLN_INFO(m_diagPrefix << "ModuleCore: coreCleanup entered");
	// Call custom module cleanup function.
	cleanup();

//	if (m_useSoa) {
//		// Calling shutdown will send out cancellation adverts, etc.
//		m_soaClient->shutdown();
//		// Reset the client shared_ptr to completely release it.
//		m_soaClient.reset();
//	}

	if (m_standaloneProcessMode) {
		// Reset node handle to ensure cleanup before calling shutdown.
		ros::shutdown();
		ros::waitForShutdown();
	}

	// Flush output to ensure we get all logs, with unknown segfault happening.
	cout << flush;
	cerr << flush;
}

// ---------------------------------------------------------------------------
bool ModuleCore::coreHandleMsgLoop(const timeout_t & waitTimeout) {
	bool allProcessed = true;
	osl_core::TimerMillis waitTimer(true /* wall timer */);
	waitTimer.setTimer(waitTimeout);

	// If not using OceanSHELL, we need to manually wait so we don't thrash the processor.
	while (waitTimer.getTimer() != 0)
	{
		// Process ROS message queues before we wait.
		ros::spinOnce();
		// TODO: May need changing when running fast simulations.
		Thread::sleep(waitTimer.getTimer());
	}

	return allProcessed;
}

// ---------------------------------------------------------------------------
void ModuleCore::run(void)
{
	PRINTLN_INFO("ModuleCore: run entered");

	m_standaloneProcessMode = true;

	if (!coreInit()) {
        PRINTLN_ERR("ModuleCore::run: coreInit returned false for module " << m_moduleName << " - exiting with error value.");
		// Flush output to ensure we get all logs, with unknown segfault happening.
		cout << flush;
		cerr << flush;
		::exit(-1);
	}

	// Loop while not stopping.
	PRINTLN_INFO("ModuleCore: Entering main loop");
	while (!isStopping())
	{
		coreHandleMsgLoop(m_minWaitTime);

		// Now check if ROS wants us to stop.
		if (!ros::ok()) {
			std::cout << "ModuleCore: Stop requested through ROS; shutting down." << std::endl;
			requestShutdown();
			// Continue skips the rest of the run loop.
			continue;
		}

		doWork();
	}

	coreCleanup();

	return;
}

// ---------------------------------------------------------------------------
void ModuleCore::requestShutdown(void)
{
	// Only print using ROS_INFO if ROS !ok.
	if (ros::ok())
	{
		PRINTLN_INFO("ModuleCore::requestShutdown called.");
	}

	m_stopRequested = true;
	m_exitValue = 0;
};

// ---------------------------------------------------------------------------
void ModuleCore::requestRestart(void)
{
	PRINTLN_INFO("ModuleCore::requestRestart called.");
	m_stopRequested = true;
	m_exitValue = -1;
};

// ---------------------------------------------------------------------------
bool ModuleCore::getINIInt(int &var, const string &name, const string &section)
{
	return m_conf.getInt(var, name, section);
}

// ---------------------------------------------------------------------------
bool ModuleCore::getINIFloat(float &var, const string &name, const string &section)
{
	return m_conf.getFloat(var, name, section);
}

// ---------------------------------------------------------------------------
bool ModuleCore::getINIString(string &var, const string &name, const string &section)
{
	return m_conf.getString(var, name, section);
}

// ---------------------------------------------------------------------------
bool ModuleCore::getINIBool(bool &var, const string &name, const string &section)
{
	return m_conf.getBool(var, name, section);
}

// ---------------------------------------------------------------------------
bool ModuleCore::isOshEnabled()
{
	return false;
}

// ---------------------------------------------------------------------------
void ModuleCore::initSignals()
{
	signal(SIGTERM, &ModuleCore::signalHandler);
	signal(SIGINT,  &ModuleCore::signalHandler);
	signal(SIGHUP,  &ModuleCore::signalHandler);
	//signal(SIGSEGV, &ModuleCore::signalHandler);
}

// ---------------------------------------------------------------------------
void ModuleCore::signalHandler(int sig)
{
	switch (sig)
	{
	case SIGTERM:
		PRINTLN_INFO("ModuleCore::signalHandler: caught TERM signal; stopping");
		ModuleCore::activeCore->requestShutdown();
		break;

	case SIGINT:
		PRINTLN_INFO("ModuleCore::signalHandler: caught INT signal; stopping");
		ModuleCore::activeCore->requestShutdown();
		break;

	case SIGHUP:
		PRINTLN_INFO("ModuleCore::signalHandler: caught HUP signal; restarting");
		ModuleCore::activeCore->requestRestart();
		break;

	default:
		PRINTLN_ERR("ModuleCore_signalHandler: unhandled signal " << sig);
		break;
	}
}

// ---------------------------------------------------------------------------
string ModuleCore::currentTimeString ()
{
	char				pstrTS[100];
	struct timeval 		stTimeVal;
#ifndef WIN32
	struct timezone		stTimeZone;
#endif
	struct tm			*pstTime;
	time_t				tTimeSec;

#ifdef WIN32
	ost::SysTime::getTimeOfDay(&stTimeVal);
#else
	gettimeofday(&stTimeVal, &stTimeZone);
#endif
	tTimeSec = stTimeVal.tv_sec;
	pstTime = localtime(&tTimeSec);

	sprintf(pstrTS, "%02d%02d%02d.%02d%02d%02d.%06d",
		(pstTime->tm_year - 100),
		(pstTime->tm_mon + 1),
		pstTime->tm_mday,
		pstTime->tm_hour,
		pstTime->tm_min,
		pstTime->tm_sec,
		(int) ((stTimeVal.tv_usec) ) );

	return string(pstrTS);
}

// ---------------------------------------------------------------------------
std::string ModuleCore::createROSNodeName(const std::string& moduleName)
{
	char hostname[31] = {0};
	gethostname(hostname, sizeof(hostname)-1);
	// Convert all characters not '[A-Za-z0-9_]' to '_'.
	size_t len = strlen(hostname);
	for (unsigned i=0; i<len; ++i) {
		if (!isalnum(hostname[i]) && hostname[i] != '_') {
			hostname[i]='_';
		}
	}			
	stringstream ss;
	ss << moduleName << "_" << hostname << "_" << getpid();
	return ss.str();
}

// ---------------------------------------------------------------------------
bool ModuleCore::checkROSClock()
{
	bool use_sim_time;
	ros::param::param("/use_sim_time", use_sim_time, false);
	// If we're using simulation time, wait for the clock to be published before going any further.
	if (use_sim_time) {
		PRINTLN_INFO("Waiting for first ROS simulation clock message.");
		while (ros::ok() && ros::Time::now().sec == 0)
			ros::WallDuration(0.01).sleep();
		if (!ros::ok()) return false;
		PRINTLN_INFO("ROS simulation clock now looks live.");
	}
	else {
		if (ros::Time::now().sec == 0) {
			PRINTLN_ERR("Param use_sim_time is not true, but time is zero. Abort!");
			return false;
		}
	}
	return true;
}

// ---------------------------------------------------------------------------
int ModuleCore::main(int argc, char * argv[], bool noWatcher)
{
	if (ModuleCore::runOnce) {
		PRINTLN_ERR("ModuleCore::main may only be run once per thread!");
		abort();
	}
	ModuleCore::runOnce = true;

	ModuleCore::activeCore = this;

	// The module name is the same as the program name; get it from args
	if (argc < 1 || argv == NULL) {
		PRINTLN_ERR("ModuleCore::main must be given argc > 0 and non-NULL argv!");
		abort();
	}
	m_moduleName = basename(argv[0]);

	m_bDaemonised = false;

	m_argc = argc;
	m_argv = argv;

	int pos = 0;
	while (++pos < argc)
	{
		if (stricmp(argv[pos], "-nowatcher") == 0)
		{
			PRINTLN_INFO("Watcher disabled.");
			noWatcher = true;
		}

		if (strcmp(argv[pos], "-d") == 0)
		{
			if (argc <= pos + 2)
			{
				cerr << "Missing daemon arguments." << endl;
				cerr << "Usage: " << argv[0] << " -d logfile lockfile" << endl;
				::exit(1);
			}

			m_logFile = argv[++pos];
			m_lockFile = argv[++pos];
			m_bDaemonised = true;
		}
	}

	int lockHandle = -1;

	if (m_bDaemonised)
	{
		int result;

		// First fork so we can go in the background.
		result=fork();
		if (result < 0)
		{
			cerr << "Error forking to make daemon." << endl;
			::exit(2);
		}
		if (result > 0) ::exit(0); // parent exits
		// child (daemon) continues

		setsid(); // obtain a new process group

		lockHandle = open(m_lockFile.c_str(), O_RDWR|O_CREAT, 0644);
		if (lockHandle < 0)
		{
			cerr << "Error opening lock file '" << m_lockFile << "', reason: " << strerror(errno) << endl;
			cerr << "Check the directory exists.";
			::exit(3);
		}
		if (lockf(lockHandle, F_TLOCK, (off_t) 0)<0)
		{
			cerr <<  "Error obtaining process lock - check if module is already running." << endl;
			::exit(4);
		}

		char pidStr[20];
		sprintf(pidStr,"%d\n",getpid());
		int writeLen = strlen(pidStr);
		result = write(lockHandle, pidStr, writeLen); /* record pid to lockfile */
		if (result < 0)
		{
			cerr << "Error writing to the PID file, reason: " << strerror(errno) << endl;
			::exit(5);
		}
		else if (result != writeLen)
		{
			cerr << "Error writing correct number of bytes to PID file." << endl;
			::exit(6);
		}

		// close all file descriptors
		for (int i=getdtablesize();i>=0;--i) ::close(i);

		// set file creation mask
		umask(022);

		// open stdin (null)
		open("/dev/null", O_RDONLY);

		// open log file for stdout
		int fh = open(m_logFile.c_str(), O_WRONLY|O_CREAT|O_TRUNC, 0644);
		if (fh < 0)
		{
			// We can't actually print anything at this point, because we have not output streams.
			//cerr << "Error opening log file '" << m_logFile << "', reason: " << strerror(errno) << endl;
			//cerr << "Check the directory exists.";
			::exit(7);
		}
		// duplicate for stderr
		result = dup(fh);
		if (result == -1)
		{
			cout << "Error on log file dup call, reason: " << strerror(errno) << endl;
			::exit(8);
		}

		// Open the lock file again, seeing as we just closed it above when redirecting stdout and stderr above.
		// TODO: Is there a better way to do this? There is a window of time where we could lose the lock.
		lockHandle = open(m_lockFile.c_str(), O_RDWR|O_CREAT, 0644);
		if (lockHandle < 0)
		{
			cerr << "Error opening lock file '" << m_lockFile << "', reason: " << strerror(errno) << endl;
			cerr << "Check the directory exists.";
			::exit(3);
		}
		if (lockf(lockHandle, F_TLOCK, (off_t) 0)<0)
		{
			cerr <<  "Error obtaining process lock - check if module is already running." << endl;
			::exit(4);
		}

		// Normally chdir("/"), but we'll stay in current directory for now.
	}

	if (!noWatcher)
	{
		PRINTLN_INFO("Running watcher.");
		ProcessWatcher::init();
	}

	// Start the thread then join it.
	start();
	join();

	// Close and delete the lockfile if it is open.
	if (lockHandle >= 0) {
		::close(lockHandle);
		unlink(m_lockFile.c_str());
	}

	PRINTLN_INFO("ModuleCore: Exiting with value " << m_exitValue);
	::exit(m_exitValue);
	// return never actually happens, because we exit above, but keep it for completeness.
	return(m_exitValue);
}

} // namespace
